#ifndef POSE_2D_H_
#define POSE_2D_H_
#include <vector>
#include <memory>

#include <ceres/ceres.h>
#include <ceres/local_parameterization.h>

#include "state.h"

class Pose2d : public ceres::LocalParameterization  {
public:
    virtual int GlobalSize() const { return 3; };
    virtual int LocalSize() const { return 3; };

    virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;
    virtual bool ComputeJacobian(const double *x, double *jacobian) const;

    static double NormalizeAngle(double angle) {
        while (angle > M_PI) angle -= 2. * M_PI;
        while (angle < -M_PI) angle += 2. * M_PI;
        return angle;
    }
};

#endif 
